#include <hidef.h>
#include <string.h>
#include "derivative.h"
#include "sci.h"
#include "lcd.h"

void sci_putchar (char c, int port)
{
  switch (port) {
    case 0:
      /* Wait until the Transmit Data Register is empty */
	    while (SCI0SR1_TDRE!=1)
	    continue;
	    SCI0DRL = c;
	    SCI0CR2_TE=1;
	    break;
    case 1:
      /* Wait until the Transmit Data Register is empty */
    	while (SCI1SR1_TDRE!=1)
    	continue;
    	SCI1DRL = c;
    	SCI1CR2_TE=1;
    	break;
    default:
      break;
  }
}

char sci_gettchar (int port)
{
	char c;
	switch (port) {
	  case 0:
	    /* Wait until the Receive Data Register is full */
	    while (SCI0SR1_RDRF!=1)
	    continue;
	    c=SCI0DRL;
	    SCI0CR2_RE=1;
	    break;
	  case 1:
	    /* Wait until the Receive Data Register is full */
	    while (SCI1SR1_RDRF!=1)
	    continue;
	    c=SCI1DRL;
	    SCI1CR2_RE=1;
	    break;
	  default:
	    c='?';
	    break;
	}
	return(c);
}

void sci_print (const char *msg, int port)
{
	while (*msg != 0 && *msg != '\n')
	sci_putchar (*msg++, port);
	sci_putchar('\n',port);
}

char *sci_scan (int port)
{
	static char msg[MAXLENGTH];
	char *count,c;
	count=msg;
	while ((c=sci_gettchar(port)) != 0x0D && count-msg < MAXLENGTH-1)
	*count++ = c;
	return(msg);
}

void print_angle(int head, int port) {
  char msg[4];
  int mid[4],i;
  mid[0] = (head/1000);
  mid[1] = (head/100)-(mid[0]*10);
  mid[2] = (head/10)-(mid[1]*10)-(mid[0]*100);
  mid[3] = head-(mid[2]*10)-(mid[1]*100)-(mid[0]*1000);
  for (i=0;i<4;i++) {
    msg[i]=mid[i]+'0';  
  }
  sci_putchar(msg[0],port);
  sci_putchar(msg[1],port);
  sci_putchar(msg[2],port);
  sci_putchar('.',port);
  sci_putchar(msg[3],port);
  sci_putchar(0x0D,port);
  sci_putchar(0x0A,port);  
}

void sci_init (void)
{
	/* Initialize SCI0 */
	SCI0BDL = SCI0BAUD;
	SCI0CR1 = 0;
	SCI0CR2_TE = 1;
	SCI0CR2_RE = 1;
	SCI0CR2_RIE = 1;

	/* Initialize SCI1 */
	SCI1BD = SCI1BAUD;
	SCI1CR1 = 0;
	SCI1CR2_TE = 1;
	SCI1CR2_RE = 1;
	SCI1CR2_RIE = 1;
}

void copystr(volatile unsigned char* src, volatile unsigned char* dest, int length) {
  int i;
  for (i=0; i<length; i++) {
    dest[i]=src[i];
  }
}

void zero(volatile unsigned char* dest, int length) {
  int i;
  for (i=0; i<length; i++) {
    dest[i]=0;
  }
}

